/*
 * File:    VectorMath.c
 * Project:	HobbyPilotFirmware (HPF)
 * Author:  Wouter van Verre
 *
 * Created on February 20, 2011, 3:09 PM
 */

#include "Header.h"

void v3_VectorAdd(float * Vect1, float * Vect2, float * Dest)	// Vect1 + Vect2 = Dest
{
	Dest[0] = Vect1[0] + Vect2[0];
	Dest[1] = Vect1[1] + Vect2[1];
	Dest[2] = Vect1[2] + Vect2[2];
}

void v3_VectorScale(float * Vect, float S, float * Dest) // Vect * S = Dest
{
	Dest[0] = Vect[0] * S;
	Dest[1] = Vect[1] * S;
	Dest[2] = Vect[2] * S;
}

void v3_VectorDotProduct(float * Vect1, float * Vect2, float * Dest)
{	
	*Dest = ((Vect1[0] * Vect2[0]) + (Vect1[1] * Vect2[1]) + (Vect1[2] * Vect2[2]));
}

void v3_VectorCrossProduct(float * Vect1, float * Vect2, float * Dest)
{
	Dest[0] = Vect1[1] * Vect2[2] - Vect1[2] * Vect2[1];
	Dest[1] = Vect1[2] * Vect2[0] - Vect1[0] * Vect2[2];
	Dest[2] = Vect1[0] * Vect2[1] - Vect1[1] * Vect2[0];
}

void v3_GetRowAsVector(float * Matrix, float * Dest)
{
	Dest[0] = Matrix[0];
	Dest[1] = Matrix[1];
	Dest[2] = Matrix[2];
}
